package Code;
import java.util.List;
import java.util.Random;


public enum CalculatePotentials 
{;
	private static double minDistance = 2;
	private static double obstacleRadiusOfInfluence = 35;
	private static double goalRadiusOfInfluence = 32;
	
	/************************************************************
	 * Calculate the potential field for a given pt and goal
	 * 		Returns the new point to move toward
	 * @param currPt the current pt
	 * @param goal the goal
	 * @param obstacles a list of obstacles on the field
	 * @return the potential (x & y) as a point
	 **********************************************************/
	public static Point calcPotentialPt(Point currPt, Point goal, List<Obstacle> obstacles, int worldDimensions) 
	{
		//calc the attraction
		double x_attraction = calcAttraction(currPt, goal).getX();
		double y_attraction = calcAttraction(currPt, goal).getY();
		Point resistance = calcResistance(currPt, obstacles);
		double x_resistance = resistance.getX();
		double y_resistance = resistance.getY();
		Point tangential = calcTangential(currPt, obstacles, goal);
		double x_tangential = tangential.getX();
		double y_tangential = tangential.getY();
		
		double x_random = 0;
		double y_random = 0;
		if (Math.abs(x_resistance) > 0.001 || Math.abs(y_resistance) > 0.001)
		{
			Point random = calcRandom(currPt, worldDimensions);
			x_random = random.getX();
			y_random = random.getY();
		}
		
		return new Point(x_attraction + x_resistance + x_tangential + x_random + currPt.getX(), 
							y_attraction + y_resistance + y_tangential + y_random + currPt.getY());
	}
	
	/***************************************************************
	 * Calculate the resistance field for a location
	 * @param currPt	the current location
	 * @param obstacles	the obstacles on the field
	 * @return	the resistance in the x direction
	 *****************************************************************/
	public static Point calcResistance(Point currPt, List<Obstacle> obstacles)
	{
		double thetaObstacle;
		double distToObstacle;
		Point obstacleClosestPt;
		double x_resistance = 0;
		double y_resistance = 0;
		for (Obstacle obst : obstacles)
		{
			obstacleClosestPt = obst.calcClosesPt(currPt);				//closest pt on obstacle
			
			thetaObstacle = calcAngle(currPt, obstacleClosestPt);		//theta for obstacleClosestPt
			distToObstacle = Math.max(obstacleClosestPt.calcDistance(currPt), minDistance);
			if (distToObstacle <= obstacleRadiusOfInfluence)
			{
				x_resistance += (distToObstacle - obstacleRadiusOfInfluence)*Math.cos(thetaObstacle);
				y_resistance += (distToObstacle - obstacleRadiusOfInfluence)*Math.sin(thetaObstacle);
			}
		}
		return new Point(x_resistance, y_resistance);
	}
	
	/****************************************************************
	 * Calculate the attraction field for a given point and goal
	 * @param currPt	current location
	 * @param goal		the goal
	 * @return			the attraction at a given pt
	 **************************************************************/
	public static Point calcAttraction(Point currPt, Point goal)
	{
		double thetaGoal = calcAngle(currPt, goal);
		double distToGoal = Math.max(currPt.calcDistance(goal), minDistance);
		if (distToGoal <= goalRadiusOfInfluence)
		{
			return new Point(distToGoal*Math.cos(thetaGoal), distToGoal*Math.sin(thetaGoal));
		}
		else
			return new Point (goalRadiusOfInfluence*Math.cos(thetaGoal), goalRadiusOfInfluence*Math.sin(thetaGoal));
	}
	
	/******************************************************************
	 * Calculate the tangential field for a given point
	 * @param currPt	the current location
	 * @param obstacles	the obstacles on the field
	 * @return		the attraction at the given pt
	 ******************************************************************/
	public static Point calcTangential(Point currPt, List<Obstacle> obstacles, Point goal)
	{
		double thetaGoal = calcAngle(currPt, goal);
		double x_tangential = 0;
		double y_tangential = 0;
		double thetaObstacle;
		double distToObstacle;
		Point obstacleClosestPt;
		for (Obstacle obst : obstacles)
		{
			obstacleClosestPt = obst.calcClosesPt(currPt);			//closest pt on obstacle
			thetaObstacle = calcAngle(currPt, obstacleClosestPt);	//theta for obstacleClosestPt
			distToObstacle = Math.max(obstacleClosestPt.calcDistance(currPt), minDistance);
			thetaObstacle = adjustTheta(thetaGoal, thetaObstacle);
			if (distToObstacle <= obstacleRadiusOfInfluence)
			{
				x_tangential += (distToObstacle - obstacleRadiusOfInfluence)*Math.cos(thetaObstacle);
				y_tangential += (distToObstacle - obstacleRadiusOfInfluence)*Math.sin(thetaObstacle);
			}
		}	
		
		return new Point (x_tangential, y_tangential);
	}
	
	private static double adjustTheta(double thetaGoal, double thetaObstacle) 
	{
		
//		if (Math.abs(thetaGoal) < Math.PI/4)
//		{
//			System.out.println("Goal < pi/2");
//			thetaObstacle -= Math.PI/2;		//adjust theta for tangential calculation
//		}
//		else {
//			System.out.println("Goal >= pi/2");
//			thetaObstacle += Math.PI/2;		//adjust theta for tangential calculation
//		}
		if (thetaObstacle < Math.PI)
		{
			thetaObstacle -= Math.PI/2;		//adjust theta for tangential calculation
		}
		else {
			thetaObstacle += Math.PI/2;		//adjust theta for tangential calculation
		}
		return thetaObstacle;
	}


	/*******************************************************************
	 * Calculate the angle between 2 points
	 * @param currPt  the first point
	 * @param goal    the second point
	 * @return		  the angle between 2 pts
	 ******************************************************************/
	public static double calcAngle(Point currPt, Point goal) 
	{
		return Math.atan2(goal.getY() - currPt.getY(), goal.getX()- currPt.getX());
	}
	
	public static Point calcRandom(Point currPt, int worldDimension)
	{
		Random rand = new Random();
		int x = rand.nextInt(worldDimension) - worldDimension/3;
		int y = rand.nextInt(worldDimension) - worldDimension/3;
		Point goal = new Point (x, y);
		x = 0;
		y = 0;
		double theta = calcAngle(currPt, goal);
		double distToGoal = Math.max(currPt.calcDistance(goal), minDistance);
		if (distToGoal <= obstacleRadiusOfInfluence/2)
		{
			return new Point(distToGoal*Math.cos(theta), distToGoal*Math.sin(theta));
		}
		else
			return new Point (0, 0);
	}
}
